#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>

#include <rosbag/bag.h>
#include <rosbag/view.h>

#include "../camera_models/include/EquidistantCamera.h"
#include "../camera_models/include/PinholeCamera.h"
#include "LaseCamCalCeres.h"
#include "calcCamPose.h"
#include "config.h"
#include "selectScanPoints.h"
#include "utilities.h"
#include <Eigen/Core>

template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
    std::cout << name << std::endl;
    T ans;
    if (n.getParam(name, ans)) {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    } else {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "LaserCamCal");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");
    int ret = 0;
    bool valid_flag = false;

    std::string config_file;
    config_file = readParam<std::string>(pnh, "config_file");
    readParameters(config_file);

    rosbag::Bag bag_input;
    bag_input.open(bag_path, rosbag::bagmode::Read);
    std::vector<std::string> topics;
    topics.push_back(scan_topic_name);
    topics.push_back(img_topic_name);
    rosbag::View views(bag_input, rosbag::TopicQuery(topics));

    // Load apritag pose
    std::vector<CamPose>      vTagpose;
    std::vector<CamPoseValid> vValidTagPose;
    LoadCamPoseFromTxt(savePath + "apriltag_pose.txt", vTagpose);
    std::cout << "Load apriltag pose size: " << vTagpose.size() << std::endl;

    if (vTagpose.size() < 10) {
        std::cout << "apriltag pose less than 10." << std::endl;
        return 0;
    }

    for (rosbag::MessageInstance const m : views) {
        if (m.getTopic() != img_topic_name) {
            continue;
        }

        sensor_msgs::ImageConstPtr img = m.instantiate<sensor_msgs::Image>();
        double                     img_timestamp = img->header.stamp.toSec();
        cv_bridge::CvImagePtr      cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::BGR8);
        cv::Mat img_raw = cv_ptr->image.clone();
        if (img_raw.channels() == 3) {
            cv::cvtColor(img_raw, img_raw, CV_BGR2GRAY);
        }
        cv::imshow("tag time IMAGE", img_raw);
        ret = cv::waitKey(80);
        if (ret == 't') {
            valid_flag = !valid_flag;
            std::cout << "valid_flag:" << valid_flag << std::endl;
        }

        for (auto pose : vTagpose) {
            if (pose.timestamp == img_timestamp) {
                CamPoseValid validPose;
                validPose.timestamp = pose.timestamp;
                validPose.start_time = pose.start_time;
                validPose.end_time = pose.end_time;
                validPose.qwc = pose.qwc;
                validPose.twc = pose.twc;
                validPose.valid_flag = valid_flag;
                vValidTagPose.push_back(validPose);
            }
        }

    }

    std::string   file = savePath + "apriltag_pose_valid.txt";
    SaveCamPoseValidtoTxt(file, vValidTagPose);

    ros::spin();
}
